#!/usr/bin/env python3
"""
Launch Gazebo with the RobotOne arm, ros2_control and controllers.
"""

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, RegisterEventHandler, TimerAction
from launch.event_handlers import OnProcessExit
from launch.substitutions import (
    LaunchConfiguration,
    PathJoinSubstitution,
    Command,
    FindExecutable,
)
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    world_arg = DeclareLaunchArgument(
        "world",
        default_value="empty.world",
        description="Gazebo world file (located in gazebo_ros package by default)",
    )
    use_sim_time_arg = DeclareLaunchArgument(
        "use_sim_time",
        default_value="true",
        description="Use simulation time",
    )
    gui_arg = DeclareLaunchArgument(
        "gui",
        default_value="false",
        description="Launch Gazebo client (set false for headless servers)",
    )

    pkg_share = FindPackageShare("arm")
    xacro_file = PathJoinSubstitution([pkg_share, "urdf", "robotone_arm.urdf.xacro"])
    robot_description_content = Command([FindExecutable(name="xacro"), " ", xacro_file])
    robot_description = {"robot_description": robot_description_content}

    control_config = PathJoinSubstitution([pkg_share, "config", "ros2_control.yaml"])

    gazebo_pkg = FindPackageShare("gazebo_ros")
    world_path = PathJoinSubstitution([gazebo_pkg, "worlds", LaunchConfiguration("world")])
    gazebo_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution([gazebo_pkg, "launch", "gazebo.launch.py"])
        ),
        launch_arguments={
            "world": world_path,
            "gui": LaunchConfiguration("gui"),
            "verbose": "false",
        }.items(),
    )

    robot_state_pub = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        parameters=[robot_description, {"use_sim_time": LaunchConfiguration("use_sim_time")}],
    )

    ros2_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[robot_description, control_config],
        output="screen",
    )

    spawn_entity = Node(
        package="gazebo_ros",
        executable="spawn_entity.py",
        arguments=["-entity", "robotone_arm", "-topic", "robot_description"],
        output="screen",
    )

    js_broadcaster = Node(
        package="controller_manager",
        executable="spawner",
        arguments=[
            "joint_state_broadcaster",
            "--controller-manager",
            "/controller_manager",
            "--controller-manager-timeout",
            "120",
        ],
        output="screen",
    )
    traj_controller = Node(
        package="controller_manager",
        executable="spawner",
        arguments=[
            "arm_joint_trajectory_controller",
            "--controller-manager",
            "/controller_manager",
            "--controller-manager-timeout",
            "120",
        ],
        output="screen",
    )

    controller_spawners = RegisterEventHandler(
        OnProcessExit(
            target_action=spawn_entity,
            on_exit=[
                TimerAction(period=2.0, actions=[js_broadcaster]),
                TimerAction(period=5.0, actions=[traj_controller]),
            ],
        )
    )

    return LaunchDescription(
        [
            world_arg,
            use_sim_time_arg,
            gui_arg,
            gazebo_launch,
            robot_state_pub,
            ros2_control_node,
            spawn_entity,
            controller_spawners,
        ]
    )

